package impl

import (
	"encoding/json"
	"iron_man_comm/src/goroslib"
	"iron_man_comm/src/goroslib/pkg/msgs/sensor_msgs"
	"iron_man_comm/src/tools/log"
	"math"
)

type CheckLds struct {
	SensorCheckData
}

func (sensor *CheckLds) CheckData() error {
	var err error
	sensor.subscriber, err = goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:      sensor.node,
		Topic:     "/scan",
		Callback:  sensor.onCallback,
		QueueSize: 10,
	})
	if nil != err {
		return err
	}
	log.Info.Println("subscribed to [/scan]")
	sensor.EnableTimer(true)
	return nil
}

func (sensor *CheckLds) onCallback(msg *sensor_msgs.LaserScan) {
	sensor.calcTime()

	if ModeData == sensor.mode { //需要打印
		sensor.formatNaN(msg)
		bytes, err := json.Marshal(*msg)
		if nil != err {
			log.Error.Println("lds msg to json fail, err: ", err.Error())
			return
		}
		log.Info.Println("-------------------------------------------------------------")
		log.Info.Printf("%s", string(bytes))
	}
}

func (sensor *CheckLds) formatNaN(msg *sensor_msgs.LaserScan) {
	for index, value := range msg.Ranges {
		if math.IsNaN(float64(value)) {
			msg.Ranges[index] = -1
		}
	}
	for index, value := range msg.Intensities {
		if math.IsNaN(float64(value)) {
			msg.Intensities[index] = -1
		}
	}
}
